#include <FastSerial.h>
#define CAMERAPORT Serial2
#define DEBUGPORT Serial
#define BUFFERSIZE 32
FastSerialPort0(DEBUGPORT);
FastSerialPort2(CAMERAPORT);

#define LEDPIN 13

     

void setup()
{
  pinMode(LEDPIN, OUTPUT);  //LED debug
  DEBUGPORT.begin(57600);
  CAMERAPORT.begin(9600);
} 
  
void loop()
{
  
  //Misc variables
  byte i=0;
  byte j=0;
  byte CAMERAbuffer[BUFFERSIZE];
  byte DEBUGbuffer[BUFFERSIZE];
  byte c;
  int light=LOW;
  unsigned long t=millis();
  CAMERAbuffer[0]=0xF8;
  CAMERAbuffer[1]=0x30;
  CAMERAbuffer[2]=0x01;
  CAMERAbuffer[3]=0xFF;
  CAMERAPORT.write(CAMERAbuffer,4);
  CAMERAbuffer[0]=0x81;
  CAMERAbuffer[1]=0x01;
  CAMERAbuffer[2]=0x00;
  CAMERAbuffer[3]=0x01;
  CAMERAbuffer[4]=0xFF;
  CAMERAPORT.write(CAMERAbuffer,5);
  
  while(1){
    
    if (DEBUGPORT.available()>0) {
      DEBUGbuffer[i++]=DEBUGPORT.read();
      if (DEBUGbuffer[i-1]==0xFF) {
        CAMERAPORT.write(DEBUGbuffer,i);
        DEBUGPORT.write(DEBUGbuffer,i);
        DEBUGPORT.print("\t");
        i=0;
      }
    }

    if (CAMERAPORT.available()>0) {
      CAMERAbuffer[j++]=CAMERAPORT.read();
      if (CAMERAbuffer[j-1]==0xFF) {
        DEBUGPORT.write(CAMERAbuffer,j);
        j=0;
      }
    }    
    
    if( (millis()-t) > 100) { // Main loop runs at 10Hz
      t=millis();
      light=!light;
      digitalWrite(13, light);   // set the LED on
      //DEBUGPORT.println(CAMERAPORT.available());
    }//end 10Hz
    
  }//end while(1)
}//end loop()








